#pragma once

#ifndef _FRAME_H_
#define _FRAME_H_

#include "common_include.h"
#include"utility.h"

namespace vld_imu_slam{

struct Frame{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    typedef shared_ptr<Frame> Ptr;

    unsigned long keyframe_id_ =0;
    unsigned long frame_id_ =0;
    double time_stamp_;
    bool is_keyframe_ = false;
    bool is_get_imu_pose_estimate_ = false;

    SE3 w_pose_,imu_pose_estimate_;

    pcl::PointCloud<PointType>::Ptr pc_surface_,pc_corner_,pc_full_,pc_surface_sample_,pc_corner_sample_;

    mutex w_pose_mutex_,imu_pose_mutex_;

    Frame(){}
    Frame(double time_stamp,pcl::PointCloud<PointType>::Ptr pc,pcl::PointCloud<PointType>::Ptr pc_plane,pcl::PointCloud<PointType>::Ptr pc_corner){
        time_stamp_ = time_stamp;
        pc_corner_ =pc_corner;
        pc_surface_ = pc_plane; 
        pc_full_ = pc;
        pc_surface_sample_.reset(new pcl::PointCloud<PointType>);
        pc_corner_sample_.reset(new pcl::PointCloud<PointType>);
        pcl::copyPointCloud(*pc_corner_,*pc_corner_sample_);
        pcl::copyPointCloud(*pc_surface_,*pc_surface_sample_);
        pcDownSample(pc_full_, 0.5);
        pcDownSample(pc_surface_sample_, 1.0);
        pcDownSample(pc_corner_sample_, 0.2);
        
        // pcDownSample(pc_ground_,0.2);

        w_pose_ = SE3(Eigen::Quaterniond(1,0,0,0),Eigen::Vector3d(0,0,0));
        imu_pose_estimate_ = w_pose_;

        static long frame_factory_id = 0;
        frame_id_ = frame_factory_id++;
    }

    void setKeyFrame(){
        static long keyframe_factory_id = 0;
        is_keyframe_ = true;
        keyframe_id_ = keyframe_factory_id++;
    }

    SE3 getPose(){
        unique_lock<mutex> lck(w_pose_mutex_);
        if(is_get_imu_pose_estimate_){
            return getImuPose();
        }else{
            return w_pose_;
        }
    }

    void setPose(const SE3 &pose) {
        unique_lock<mutex> lck(w_pose_mutex_);
        if(is_get_imu_pose_estimate_){
            double imu_pose_array[7];
            SE3toRt(getImuPose(),imu_pose_array);
            Eigen::Vector3d t_imu(imu_pose_array[4],imu_pose_array[5],imu_pose_array[6]);
            Eigen::Quaterniond q_imu(imu_pose_array[3],imu_pose_array[0],imu_pose_array[1],imu_pose_array[2]);
            double lidar_pose_array[7];
            SE3toRt(pose,lidar_pose_array);
            Eigen::Vector3d t_lidar(lidar_pose_array[4],lidar_pose_array[5],lidar_pose_array[6]);
            Eigen::Quaterniond q_lidar(lidar_pose_array[3],lidar_pose_array[0],lidar_pose_array[1],lidar_pose_array[2]);
            SO3 q_delta = (q_lidar.inverse()*q_imu).matrix();
            t_lidar = 0.5*(t_imu+t_lidar);
            q_lidar = q_lidar*Sophus::SO3d::exp(q_delta.log()*0.5).matrix();
            w_pose_ = SE3(q_lidar,t_lidar);
        }else{
            w_pose_ = pose;
        }
        is_get_imu_pose_estimate_ = false;
    }

    void setImuPose(const SE3 &pose) {
        unique_lock<mutex> lck(imu_pose_mutex_);
        imu_pose_estimate_ = pose;
        is_get_imu_pose_estimate_ = true;
    }

    SE3 getImuPose() {
        unique_lock<mutex> lck(imu_pose_mutex_);
        return imu_pose_estimate_;
    }

    PointType getFramePosition(){
        PointType pt;
        SE3 pose = getPose();
        pt.x = pose.translation()[0];
        pt.y = pose.translation()[1];
        pt.z = pose.translation()[2];
        return pt;
    }

};



}
#endif  
